import OSC
import time, threading, sys, os
import visual, visual.controls

ACC = 0
FORCE = 1

values = {'fx':0, 'fy':0, 'fz':0, 'ax':0, 'ay':0, 'az':0}

########################################################################
####################### Start the support software #####################

# Launch glovepie with the wiimote script, run, and minimize to tray.
os.system("start GlovePIE3\\GlovePIE.exe -wiimote.pie /tray")
receive_address = '127.0.0.1', 9999 # Define the address to listen on.
s = OSC.OSCServer(receive_address) # Launch the OpenSoundControl Server.
s.addDefaultHandlers() # this registers the default handlers

#### Start adding handlers
def acc_x(addr, tags, stuff, source):
    values['ax'] = stuff
    arrow_accel_x.axis = (0, stuff[0]*10, 0)
    label_accel_x.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/accel/x", acc_x) # adding our function

def acc_y(addr, tags, stuff, source):
    values['ay'] = stuff
    arrow_accel_y.axis = (0, stuff[0]*10, 0)
    label_accel_y.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/accel/y", acc_y) # adding our function

def acc_z(addr, tags, stuff, source):
    values['az'] = stuff
    arrow_accel_z.axis = (0, stuff[0]*10, 0)
    label_accel_z.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/accel/z", acc_z) # adding our function

def force_x(addr, tags, stuff, source):
    values['fx'] = stuff
    label_accel_x.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/force/x", force_x) # adding our function

def force_y(addr, tags, stuff, source):
    values['fy'] = stuff
    label_accel_y.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/force/y", force_y) # adding our function

def force_z(addr, tags, stuff, source):
    values['fz'] = stuff
    label_accel_z.label = '%.3f' % stuff[0]
    print stuff
s.addMsgHandler("/force/z", force_z) # adding our function

# just checking which handlers we have added
print "Registered Callback-functions are :"
for addr in s.getOSCAddressSpace():
    print addr

########################################################################
###################### Start visualisation code ########################

#       +Y
#        |
#        |
#        |
#       0|_______ +X
#       /
#      /
#     /
#   +Z

chart = visual.display(
    title = "WiiPhysics V0.3 - Charting Display",
    width = 800,
    height = 600,
    center = (5,0,0)
)
chart.autoscale = 0
chart.range = (1000,1000,1000)

situation = visual.display(
    title = "WiiPhysics V0.3 - Situational Display",
    width = 800,
    height = 600,
    center = (0,0,0)
)

### Situation
arrow_accel_x = visual.arrow(
    display = situation,
    pos = (0,0,0),
    axis = (0,0,0),
    shaftwidth = 10,
    color = (1,0,0)
)
arrow_accel_y = visual.arrow(
    display = situation,
    pos = (0,0,0),
    axis = (0,0,0),
    shaftwidth = 10,
    color = (0,1,0)
)
arrow_accel_z = visual.arrow(
    display = situation,
    pos = (0,0,0),
    axis = (0,0,0),
    shaftwidth = 10,
    color = (0,0,1)
)

### Chart
label_forces = visual.label(
    display = chart,
    pos = (-500, 100, 0),
    text = "Forces"
)
label_forces_x = visual.label(
    display = chart,
    pos = (-300, 100, 0)
)
label_forces_y = visual.label(
    display = chart,
    pos = (0, 100, 0)
)
label_forces_z = visual.label(
    display = chart,
    pos = (300, 100, 0)
)

label_accel = visual.label(
    display = chart,
    pos=(-500, -100, 0),
    text="Acceleration"
)
label_accel_x = visual.label(
    display = chart,
    pos = (-300, -100, 0)
)
label_accel_y = visual.label(
    display = chart,
    pos = (0, -100, 0)
)
label_accel_z = visual.label(
    display = chart,
    pos = (300, -100, 0)
)

##############################################################
##################### Start the OSC Server ###################
#################### and visualisation code ##################

print "\nStarting OSCServer. Use ctrl-C to quit."
server_thread = threading.Thread( target = s.serve_forever )
server_thread.start()
print "Running"

try :
    while 1 :
        visual.rate(100)
        #con.interact()

except KeyboardInterrupt :
    print "\nClosing OSCServer."
    s.close()
    print "Waiting for Server-thread to finish"
    server_thread.join() # (hope like hell its quick!)
    print "Done"
    time.sleep(3)
    sys.exit()
